#include "ConvexShape2.h"
#include "MinkowskiSum2.h"
#include "Collision2.h"
#include "Allocator.h"
#include "DebugRendererGL.h"
#include <GL/glut.h>
#include <iostream>


static Vector2	diskCenter1(-0.5, 0);
static const Scalar DELTA = 0.001;
static Vector2 sVelocity;
static bool isContinuous = false;

static void toggleCCD()
{
	isContinuous = !isContinuous;
	if (isContinuous)
		std::cout << "Continuous Collision Detection\n";
	else
		std::cout << "Discrete Collision Detection\n";
}

void keyboard(unsigned char key, int x, int y)
{
	switch (key) {
	case 'q':
	case 'Q':
	case '\033':  //ESC
		std::exit(0);
	case 'c':
	case 'C':
		toggleCCD();
	default:
		break;
	}
}

void specialKey(int key, int x, int y)
{
	switch (key) {
	case GLUT_KEY_RIGHT:
		sVelocity.setX(sVelocity.x() + DELTA);
		break;
	case GLUT_KEY_LEFT:
		sVelocity.setX(sVelocity.x() - DELTA);
		break;
	case GLUT_KEY_UP:
		sVelocity.setY(sVelocity.y() + DELTA);
		break;
	case GLUT_KEY_DOWN:
		sVelocity.setY(sVelocity.y() - DELTA);
		break;
	default:
		break;
	}
}

void moveDisk1()
{
	glutPostRedisplay();
}

void drawCollision(const IConvexDomain2& domainA, const Vector2& pA0, const Vector2& pA1, const IConvexDomain2& domainB, const Vector2& pB)
{
	static const Scalar TOLERANCE = 0.0002;

	Vector2 cpA, cpB, cnA;
	bool isIntersect;
	if (isContinuous)
	{
		Scalar lambda = 0;
		isIntersect = computeTimeOfImpact(domainA, pA0, pA1, domainB, pB, pB, TOLERANCE, cpA, cpB, cnA, lambda);
		if (isIntersect)
		{
			std::cout << "first time of impact is " << lambda << std::endl;

			Vector2 pAlambda = pA0 + (pA1 - pA0) * lambda;
			::glBegin(GL_LINES);
			::glColor3f(0, 0, 1);
			::glVertex2d(pA0.x(), pA0.y());
			::glVertex2d(pAlambda.x(), pAlambda.y());
			::glEnd();
		}
	}
	else
	{
		Scalar  d = 0;
		isIntersect = findContactPoint(domainA, pA1, domainB, pB, TOLERANCE, cpA, cpB, cnA, d);
		if (isIntersect)
		{
			Scalar normalDistance = 0.1;
			::glBegin(GL_LINES);
			::glColor3f(0, 1, 0);
			::glVertex2d(cpA.x(), cpA.y());
			::glVertex2d(cpA.x() + cnA.x() * normalDistance, cpA.y() + cnA.y() * normalDistance);
			::glVertex2d(cpB.x(), cpB.y());
			::glVertex2d(cpB.x() - cnA.x() * normalDistance, cpB.y() - cnA.y() * normalDistance);
			::glEnd();
		}
	}
	if (isIntersect)
	{
		// draw segment between both contact points.
		::glColor3f(1, 0, 0);
		::glBegin(GL_LINES);
		::glVertex2d(cpA.x(), cpA.y());
		::glVertex2d(cpB.x(), cpB.y());
		::glEnd();
		::glPointSize(5.);
		::glBegin(GL_POINTS);
		::glVertex2d(cpA.x(), cpA.y());
		::glVertex2d(cpB.x(), cpB.y());
		::glEnd();
	}
}

void testCollision()
{
	static StackAllocator stack(0x1 << 20);	//1MB
	
	DebugRendererGL renderer(0.005, stack);

	// controllable disk
	Disk2					disk1(0.3);
	glColor3f(0, 0, 1);
	renderer.render(disk1, diskCenter1);


	// disk
	Disk2					disk2(0.1);
	Vector2					diskCenter2(-0.2, 0.38);

	glColor3f(0, 0, 0);
	renderer.render(disk2, diskCenter2);
	
	// polygon
	ConvexPolygon2			polygon;
	{
		polygon.addVertex(Vector2(0, 0.5));
		polygon.addVertex(Vector2(0, -0.5));
		polygon.addVertex(Vector2(0.5, 0));
		polygon.addVertex(Vector2(0.5, 0.8));
	}
	Vector2 polygonTransform(0, 0);

	glColor3f(0, 0, 0);
	renderer.render(polygon, polygonTransform);

	// sum
	MinkowskiSumExIm2 offsetPolygon(polygon, polygonTransform, disk2, diskCenter2);
	Vector2 offsetPolygonTransform(0, 0);
	glColor3f(0, 0.5, 1);
	renderer.render(offsetPolygon, offsetPolygonTransform);


	Vector2 nextDiskCenter1 = diskCenter1 + sVelocity;
	//drawCollision(disk1, diskCenter1, nextDiskCenter1, disk2, diskCenter2);
	drawCollision(disk1, diskCenter1, nextDiskCenter1, polygon, polygonTransform);
	//drawCollision(disk1, diskCenter1, nextDiskCenter1, offsetPolygon, offsetPolygonTransform);
	diskCenter1 = nextDiskCenter1;
}

//////////////////////////////////////////////////

void display(void)
{
	glClear(GL_COLOR_BUFFER_BIT);
	
	testCollision();

	glColor3f(0, 0, 0.3);

	//frame
	glBegin(GL_LINE_LOOP);
	glVertex2d(-0.9, -0.9);
	glVertex2d(0.9, -0.9);
	glVertex2d(0.9, 0.9);
	glVertex2d(-0.9, 0.9);
	glEnd();
	
	glutSwapBuffers();
}

int main(int argc, char *argv[])
{
	glutInit(&argc, argv);
	glutInitDisplayMode(GLUT_RGBA | GLUT_DOUBLE);
	glutInitWindowSize(500, 500);
	glutCreateWindow(argv[0]);
	glutDisplayFunc(display);
	glutKeyboardFunc(keyboard);
	glutSpecialFunc(specialKey);
	glutIdleFunc(moveDisk1);
	glClearColor(1.0, 1.0, 1.0, 1.0); 
	glutMainLoop();
	return 0;

}
